/**
  ******************************************************************************
  * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
  * @file    ControlFSM.cpp
  * @author
  * @brief   Code for .
  * @date   2022-02-28
  * @version 1.0
  * @par Change Log:
  * <table>
  * <tr><th>Date        <th>Version  <th>Author     <th>Description

  * </table>
  *
  ******************************************************************************
  * @attention
  *
  * if you had modified this file, please make sure your code does not have many
  * bugs, update the version Number, write dowm your name and the date, the most
  * important is make sure the users will have clear and definite understanding
  * through your new brief.
  *
  * <h2><center>&copy; Copyright (c) 2022 - ~,SCUT-RobotLab Development Team.
  * All rights reserved.</center></h2>
  ******************************************************************************
  */
#include "ControlFSM.h"

/**
 * Constructor for the Control FSM. Passes in all of the necessary
 * data and stores it in a struct. Initializes the FSM with a starting
 * state and operating mode.
 * （1）传入所有必要的数据并将其存储在结构中。使用启动状态和操作模式初始化FSM。
 * （2）传入参数从robotrunner的给RobotController* _robot_ctrl赋值来
 * @param _quadruped the quadruped information 四足的信息
 * @param _legController interface to the leg controllers 与腿部控制器的接口
 * @param _gaitScheduler controls scheduled foot contact modes 步态规划器，控制预定的足部接触模式
 * @param _desiredStateCommand gets the desired COM state trajectories 所需的COM状态轨迹
 * @param controlParameters passes in the control parameters from the GUI 控制参数
 * @param UserParameters    用户参数
 */
template <typename T>
ControlFSM<T>::ControlFSM(Quadruped<T>* _quadruped, StateEstimatorContainer<T>* _stateEstimator,
                          LegController<T>* _legController, GaitScheduler<T>* _gaitScheduler,
                          DesiredStateCommand<T>* _desiredStateCommand, RobotControlParameters* controlParameters,
                          UserParameters* userParameters) {
    // Add the pointers to the ControlFSMData struct
    //将指针形参赋值到data结构体
    data._quadruped = _quadruped;
    data._stateEstimator = _stateEstimator;
    data._legController = _legController;
    data._gaitScheduler = _gaitScheduler;
    data._desiredStateCommand = _desiredStateCommand;
    data.controlParameters = controlParameters;
    data.userParameters = userParameters;

    // Initialize and add all of the FSM States to the state list
    //初始化相关FSM状态列表
    statesList.invalid = nullptr;    //（1）初始化invalid->FSM是否使用状态数据及相关控制器
    statesList.passive = new FSM_State_Passive<T>(&data);    //（2）初始化passive->被人为操纵调试状态数据及相关控制器
    statesList.jointPD = new FSM_State_JointPD<T>(&data);    //（3）初始化jointPD->关节PD控制器状态数据及相关控制器
    // statesList.impedanceControl = new FSM_State_ImpedanceControl<T>(&data);
    // //（4）初始化impedanceControl->阻抗控制状态数据及相关控制器
    statesList.standUp = new FSM_State_StandUp<T>(&data);
    // //（5）初始化standUp->站立过程状态数据及相关控制器 statesList.balanceStand = new
    // FSM_State_BalanceStand<T>(&data);         //（6）初始化balanceStand->站立平衡状态数据及相关控制器
    statesList.locomotion = new FSM_State_Locomotion<T>(
        &data);    //（7）初始化locomotion->运动状态数据及相关控制器 statesList.recoveryStand
    // = new FSM_State_RecoveryStand<T>(&data);       //（8）初始化recoveryStand->恢复站立状态数据及相关控制器
    // statesList.vision = new FSM_State_Vision<T>(&data); //（9）初始化vision->可视化状态数据及相关控制器
    // statesList.backflip = new FSM_State_BackFlip<T>(&data); //（10）初始化backflip->后空翻状态数据及相关控制器
    // statesList.frontJump
    statesList.frontJump = new FSM_State_FrontJump<T>(&data);    //（11）初始化frontJump->向前跳状态数据及相关控制器
    statesList.jump = new FSM_State_Jump<T>(&data);    //（11）初始化Jump->跳状态数据及相关控制器

    /*创建安全检查器，进行安全检查并限制操作*/
    safetyChecker = new SafetyChecker<T>(&data);

    // Initialize the FSM with the Passive FSM State
    //使用被动FSM状态初始化
    initialize();
}

/**
 * 功能：初始化FSM
 * Initialize the Control FSM with the default settings. SHould be set to
 * Passive state and Normal operation mode.
 */
template <typename T>
void ControlFSM<T>::initialize() {
    // Initialize a new FSM State with the control data
    //用控制数据初始化一个新的FSM状态
    currentState = statesList.passive;

    // Enter the new current state cleanly
    //进入新的当前状态
    currentState->onEnter();

    // Initialize to not be in transition
    //初始化为不处于过渡状态状态进行迭代
    nextState = currentState;

    // Initialize FSM mode to normal operation
    //初始化FSM模式为正常操作
    operatingMode = FSM_OperatingMode::NORMAL;
}

/**
 * 功能：运行状态机
 * Called each control loop iteration. Decides if the robot is safe to
 * run controls and checks the current state for any transitions. Runs
 * the regular state behavior if all is normal.
 */
template <typename T>
void ControlFSM<T>::runFSM() {
    //检测躯干的横滚角度和俯仰角度是否超过不安全的阈值
    // operatingMode = safetyPreCheck();

    //判断是否使用遥控器，若使用遥控器，使用手柄设置输入状态类型
    if (data.controlParameters->use_rc) {
        int rc_mode = data._desiredStateCommand->rcCommand->mode;
        if (rc_mode == RC_mode::RECOVERY_STAND) {
            data.controlParameters->control_mode = K_RECOVERY_STAND;
        } else if (rc_mode == RC_mode::LOCOMOTION) {
            data.controlParameters->control_mode = K_LOCOMOTION;
        } else if (rc_mode == RC_mode::QP_STAND) {
            data.controlParameters->control_mode = K_BALANCE_STAND;
        } else if (rc_mode == RC_mode::VISION) {
            data.controlParameters->control_mode = K_VISION;
        } else if (rc_mode == RC_mode::BACKFLIP || rc_mode == RC_mode::BACKFLIP_PRE) {
            data.controlParameters->control_mode = K_BACKFLIP;
        } 

        /* modes used to test */
        else if (rc_mode == RC_mode::PD_JOINT) {
            data.controlParameters->control_mode = K_JOINT_PD;
        } else if (rc_mode == RC_mode::STAND_UP) {
            data.controlParameters->control_mode = K_STAND_UP;
        } else if (rc_mode == RC_mode::FRONT_JUMP) {
            data.controlParameters->control_mode = K_FRONTJUMP;
        } else if (rc_mode == RC_mode::JUMP) {
            data.controlParameters->control_mode = K_JUMP;
        }
    }

    // Run the robot control code if operating mode is not unsafe
    /*【重点！！！】判断操作模式是否处于停止模式，决定是否启动机器人运行状态机FSM*/
    if (operatingMode != FSM_OperatingMode::ESTOP) {
        // Run normal controls if no transition is detected
        /*如果操作模式正常则运行正常控件，这段是必须运行的*/
        if (operatingMode == FSM_OperatingMode::NORMAL) {
            // Check the current state for any transition
            //获取的当前转换状态命令，判断机器人FSM状态有没有改变
            nextStateName = currentState->checkTransition();

            // Detect a commanded transition
            //若有检测到切换命令，即下一个状态不等于本状态
            if (nextStateName != currentState->stateName) {
                // Set the FSM operating mode to transitioning
                operatingMode = FSM_OperatingMode::TRANSITIONING;

                // Get the next FSM State by name
                nextState = getNextState(nextStateName);
                // Print transition initialized info
                // printInfo(1);
            } else {
                //【重点！！！】运行当前状态控制器
                //在每个控制循环迭代中执行的函数，不同状态就会对应进去不同的run()函数
                // Run the iteration for the current state normally
                currentState->run();
            }
        }

        // Run the transition code while transition is occuring
        /*如果检测到切换状态*/
        if (operatingMode == FSM_OperatingMode::TRANSITIONING) {
            //获得转换数据，进行状态数据转换操作
            transitionData = currentState->transition();

            // Check the robot state for safe operation
            safetyPostCheck();

            // Run the state transition
            //若状态转换已经完成
            if (transitionData.done) {
                // Exit the current state cleanly
                currentState->onExit();

                // Print finalizing transition info
                printInfo(2);

                // Complete the transition
                currentState = nextState;

                // Enter the new current state cleanly
                currentState->onEnter();

                // Return the FSM to normal operation mode
                //操作模式设置为正常模式
                operatingMode = FSM_OperatingMode::NORMAL;
            }
        } else {
            // Check the robot state for safe operation
            safetyPostCheck();
        }
    } else {    // if ESTOP
        currentState = statesList.passive;
        currentState->onEnter();
        nextStateName = currentState->stateName;
    }

    // Print the current state of the FSM
    printInfo(0);

    // Increase the iteration counter
    iter++;
}

/**
 * 功能：检查机器人状态是否符合安全启动操作条件。
 * Checks the robot state for safe operation conditions. If it is in
 * an unsafe state, it will not run the normal control code until it
 * is safe to operate again.
 *
 * @return the appropriate operating mode
 */
template <typename T>
FSM_OperatingMode ControlFSM<T>::safetyPreCheck() {
    // Check for safe orientation if the current state requires it
    if (currentState->checkSafeOrientation && data.controlParameters->control_mode != K_RECOVERY_STAND) {
        //如果姿态角不安全，处理逻辑如下
        if (!safetyChecker->checkSafeOrientation()) {
            operatingMode = FSM_OperatingMode::ESTOP;
            std::cout << "broken: Orientation Safety Ceck FAIL" << std::endl;
        }
    }

    // Default is to return the current operating mode
    return operatingMode;
}

/**
 * 功能：检查脚的安全位置和检查期望的安全前馈力
 * Checks the robot state for safe operation commands after calculating the
 * control iteration. Prints out which command is unsafe. Each state has
 * the option to enable checks for commands that it cares about.
 *
 * Should this EDamp / EStop or just continue?
 * Should break each separate check into its own function for clarity
 *
 * @return the appropriate operating mode
 */
template <typename T>
FSM_OperatingMode ControlFSM<T>::safetyPostCheck() {
    // Check for safe desired foot positions
    //检查脚的安全位置
    if (currentState->checkPDesFoot) {
        safetyChecker->checkPDesFoot();
    }

    // Check for safe desired feedforward forces
    if (currentState->checkForceFeedForward) {
        safetyChecker->checkForceFeedForward();
    }

    // Default is to return the current operating mode
    return operatingMode;
}

/**
 * Returns the approptiate next FSM State when commanded.
 *
 * @param  next commanded enumerated state name
 * @return next FSM state
 */
template <typename T>
FSM_State<T>* ControlFSM<T>::getNextState(FSM_StateName stateName) {
    // Choose the correct FSM State by enumerated state name
    switch (stateName) {
        case FSM_StateName::INVALID:    //不合法的状态
            return statesList.invalid;

        case FSM_StateName::PASSIVE:    //被人为操纵调试状态
            return statesList.passive;

        case FSM_StateName::JOINT_PD:    //关节PD控制器
            return statesList.jointPD;

            //   case FSM_StateName::IMPEDANCE_CONTROL://阻抗控制
            //     return statesList.impedanceControl;

        case FSM_StateName::STAND_UP:    //站立过程
            return statesList.standUp;

            //   case FSM_StateName::BALANCE_STAND:  //站立平衡
            //     return statesList.balanceStand;

        case FSM_StateName::LOCOMOTION:    //运动
            return statesList.locomotion;

            //   case FSM_StateName::RECOVERY_STAND: //恢复站立
            //     return statesList.recoveryStand;

            //   case FSM_StateName::VISION:         //可视化
            //     return statesList.vision;

            //   case FSM_StateName::BACKFLIP:       //后空翻
            //     return statesList.backflip;

        case FSM_StateName::FRONTJUMP:    //向前跳
            return statesList.frontJump;

        case FSM_StateName::JUMP:    //跳
            return statesList.jump;

        default:
            return statesList.invalid;
    }
}

/**
 * 能：定期打印控制FSM信息和重要事件，例如转换初始化和终结。
 * Prints Control FSM info at regular intervals and on important events
 * such as transition initializations and finalizations. Separate function
 * to not clutter the actual code.
 *
 * @param printing mode option for regular or an event
 */
template <typename T>
void ControlFSM<T>::printInfo(int opt) {
    switch (opt) {
        case 0:    // Normal printing case at regular intervals
            // Increment printing iteration
            printIter++;

            // Print at commanded frequency
            if (printIter == printNum) {
                std::cout << "[CONTROL FSM] Printing FSM Info...\n";
                std::cout << "---------------------------------------------------------\n";
                std::cout << "Iteration: " << iter << "\n";
                if (operatingMode == FSM_OperatingMode::NORMAL) {
                    std::cout << "Operating Mode: NORMAL in " << currentState->stateString << "\n";
                } else if (operatingMode == FSM_OperatingMode::TRANSITIONING) {
                    std::cout << "Operating Mode: TRANSITIONING from " << currentState->stateString << " to "
                              << nextState->stateString << "\n";
                } else if (operatingMode == FSM_OperatingMode::ESTOP) {
                    std::cout << "Operating Mode: ESTOP\n";
                }
                std::cout << "Gait Type: " << data._gaitScheduler->gaitData.gaitName << "\n";
                std::cout << std::endl;

                // Reset iteration counter
                printIter = 0;
            }

            // Print robot info about the robot's status
            // data._gaitScheduler->printGaitInfo();
            // data._desiredStateCommand->printStateCommandInfo();

            break;

        case 1:    // Initializing FSM State transition
            std::cout << "[CONTROL FSM] Transition initialized from " << currentState->stateString << " to "
                      << nextState->stateString << "\n"
                      << std::endl;

            break;

        case 2:    // Finalizing FSM State transition
            std::cout << "[CONTROL FSM] Transition finalizing from " << currentState->stateString << " to "
                      << nextState->stateString << "\n"
                      << std::endl;

            break;
    }
}

// template class ControlFSM<double>; This should be fixed... need to make
// RobotRunner a template
template class ControlFSM<float>;

/************************ COPYRIGHT(C) SCUT-RobotLab Development Team **************************/
